package com.rockwell.maze800.test;

import net.wimpi.modbus.ModbusException;

import com.rockwell.maze800.model.IRobotCommander;
import com.rockwell.maze800.model.IRobotSensors;
import com.rockwell.maze800.model.RobotCommand;
import com.rockwell.maze800.model.SensorID;

public class DummyLRFRobotSensors implements IRobotSensors {
	
	boolean[] m_armUpData;
	boolean[] m_frontData;
	double[] m_leftData;
	double[] m_rightData;
	double[] m_compassData;
	
	int m_frontIndex = 0;
	int m_leftIndex = 0;
	int m_rightIndex = 0;
	int m_armUpIndex = 0;
	int m_compassIndex = 0;

	public DummyLRFRobotSensors(double[][] positionData, boolean[] armUpData, double[] compassData) {
		
		int length = positionData.length;
		m_frontData = new boolean[length];
		m_leftData = new double[length];
		m_rightData = new double[length];
		
		for(int i = 0; i < length; i++) {
			m_frontData[i] = positionData[i][0] == 0? false : true;
			m_leftData[i] = positionData[i][1];
			m_rightData[i] = positionData[i][2];
		}
		
		m_armUpData = armUpData;
		m_compassData = compassData;
	}

	@Override
	public boolean hasLeftDistanceSensor() {
		return true;
	}

	@Override
	public boolean hasFrontDistanceSensor() {
		return true;
	}

	@Override
	public boolean hasRightDistanceSensor() {
		return true;
	}

	@Override
	public double getLeftDistance() {
		return m_leftData[m_leftIndex++];
	}

	@Override
	public double getRightDistance() {
		return m_rightData[m_rightIndex++];
	}

	@Override
	public float getOrientation() {
		return (float)m_compassData[m_compassIndex++];
	}

	@Override
	public void setOrientation(float angle) {
	}

	@Override
	public boolean detectFrontWall() {
		m_leftIndex++;
		m_rightIndex++;
		return m_frontData[m_frontIndex++];
	}
	
	@Override
	public boolean getFrontStatus() {
		return m_frontData[m_frontIndex++];
	}

	@Override
	public boolean isArmUp() {
		return m_armUpData[m_armUpIndex++];
	}

	@Override
	public boolean isArmDown() {
		return false;
	}

	@Override
	public boolean detectLeftWall() {
		// TODO Auto-generated method stub
		return false;
	}

	@Override
	public boolean detectRightWall() {
		// TODO Auto-generated method stub
		return false;
	}
}
